#!/usr/bin/env python

import math
import geographiclib as gl
from geographiclib.geodesic import Geodesic

#
# http://en.wikipedia.org/wiki/Geodetic_system
#
# http://geographiclib.sourceforge.net/1.8/GeoidEval.1.html
#

class Ellipsoid:
    def __init__(self, a=6378137.0, f= 1./298.257223563):
        """ """
        self.a   = a;                       # semimajor axis in meters
        self.f   = f                        # reciprocal flattening
        self.b   = self.a*(1.-self.f)       # semi-minor axis in meters
        self.e2  = 2.*f-(f**2)              # squared first eccentricity
        self.ep2 = f*(2.-f)/(1.-f)**2       # squared second eccentricity


GRS80 = Ellipsoid(a=6378137., f=1/298.257222101)
WGS84 = Ellipsoid(a=6378137., f=1/298.257223563)


class ECEF:
    """
    Earth Centered, Earth Fixed
    """
    def __init__(self, x=WGS84.a, y=0., z=0.):
        self.x, self.y, self.z = x, y, z

    def __str__(self):
        return "{:f}, {:f}, {:f}".format(self.x, self.y, self.z)

    def of_llh(self, llh):
        slon, clon = math.sin(llh.lon), math.cos(llh.lon)
        slat, clat = math.sin(llh.lat), math.cos(llh.lat)
        a_chi = WGS84.a / math.sqrt(1. - WGS84.e2*slat**2)
        self.x = (a_chi + llh.h) * clat * clon
        self.y = (a_chi + llh.h) * clat * slon
        self.z = (a_chi*(1. - WGS84.e2) + llh.h) * slat
        return self
    

class LLH:
    """
    Longitude, Latitude, Height
    """
    def __init__(self, lat=0., lon=0., h=0.):
        self.lat = lat # radians 
        self.lon = lon # radians
        self.h   = h   # meters above reference ellipsoid

    def __str__(self):
        return "{:f}, {:f}, {:f}".format(self.lat, self.lon, self.h)
    
    def of_ecef(self, ecef):
        a, b = WGS84.a, WGS84.b
        a2, b2 = a**2, b**2
        e2, ep2 = WGS84.e2, WGS84.ep2
        E2 = a2 - b2
        z2, r2 = ecef.z**2, ecef.x**2+ecef.y**2
        r = math.sqrt(r2)
        F, G = 54.*b2*z2, r2 + (1-e2)*z2 - e2*E2
        c = (e2*e2*F*r2)/(G*G*G)
        s = math.pow( (1 + c + math.sqrt(c**2 + 2*c)), 1./3.)
        s1 = 1+s+1/s
        P = F/(3*s1*s1*G*G)
        Q = math.sqrt(1+2*e2*e2*P)
        ro = -(e2*P*r)/(1+Q) + math.sqrt((a*a/2)*(1+1/Q) - ((1-e2)*P*z2)/(Q*(1+Q)) - P*r2/2)
        tmp = (r - e2*ro)*(r - e2*ro)
        U, V = math.sqrt( tmp + z2 ), math.sqrt( tmp + (1-e2)*z2 )
        zo = (b2*ecef.z)/(a*V)
        self.lat = math.atan((ecef.z + ep2*zo)/r)
        self.lon = math.atan2(ecef.y,ecef.x)
        self.h = U*(1 - b2/(a*V))
        return self


if __name__ == "__main__":
    p1 = ECEF(x=WGS84.a)
    print p1

    p2 = LLH().of_ecef(p1)
    print p2

    p3 = ECEF().of_llh(p2)
    print p3

    #https://maps.google.com/maps?q=43.237991,1.327803&num=1&t=h&vpsrc=0&ie=UTF8&z=19&iwloc=A
    ricou_llh = LLH( lat=43.237991, lon=1.327803, h=0.)
    print "Ricou LLH: ", ricou_llh

    ricou_ecef = ECEF().of_llh(ricou_llh)
    print "Ricou ECEF: ",ricou_ecef

    
